#include <iostream>
#include <algorithm>
#include <fstream>
#include <chrono>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <boost/format.hpp>

#include "ygz/Frame.h"
#include "ygz/Feature.h"
#include "ygz/Settings.h"
#include "ygz/ORBExtractor.h"
#include "ygz/ORBMatcher.h"
#include "ygz/EurocReader.h"
#include "ygz/TrackerLK.h"
#include "ygz/BackendSlidingWindowG2O.h"
#include "ygz/Viewer.h"

using namespace std;
using namespace ygz;

/***
 * 本程序测试在EUROC数据集上双目纯视觉的Tracking
 */

// 路径
string leftFolder = "/var/dataset/horizon/ov_data20170623-144647/image_0";
string rightFolder = "/var/dataset/horizon/ov_data20170623-144647/image_1";
string timeFolder = "/var/dataset/horizon/ov_data20170623-144647/times.txt";
string configFile = "./examples/Horizon2.yaml";
string gtTrajectory = "/var/dataset/horizon/ov_data20170623-144647/trajectory_tfxyzxyzw.txt";

int start_index = 0; // 起始图像的索引
int nImages = 18000;

shared_ptr<TrackerLK> pTracker = nullptr;

int main(int argc, char **argv) {
    pTracker = shared_ptr<TrackerLK>(new TrackerLK);
    pTracker->TestTrackerLK();
    return 0;
}

void TrackerLK::TestTrackerLK() {

    // Read rectification parameters
    cv::FileStorage fsSettings(configFile, cv::FileStorage::READ);
    if (!fsSettings.isOpened()) {
        cerr << "ERROR: Wrong path to settings" << endl;
        return;
    }

    cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
    fsSettings["LEFT.K"] >> K_l;
    fsSettings["RIGHT.K"] >> K_r;

    fsSettings["LEFT.P"] >> P_l;
    fsSettings["RIGHT.P"] >> P_r;

    fsSettings["LEFT.R"] >> R_l;
    fsSettings["RIGHT.R"] >> R_r;

    fsSettings["LEFT.D"] >> D_l;
    fsSettings["RIGHT.D"] >> D_r;

    int rows_l = fsSettings["LEFT.height"];
    int cols_l = fsSettings["LEFT.width"];
    int rows_r = fsSettings["RIGHT.height"];
    int cols_r = fsSettings["RIGHT.width"];

    if (K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() ||
        D_r.empty() ||
        rows_l == 0 || rows_r == 0 || cols_l == 0 || cols_r == 0) {
        cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
        return;
    }

    cv::Mat M1l, M2l, M1r, M2r;
    cv::initUndistortRectifyMap(K_l, D_l, R_l, P_l.rowRange(0, 3).colRange(0, 3), cv::Size(cols_l, rows_l), CV_32F, M1l,
                                M2l);
    cv::initUndistortRectifyMap(K_r, D_r, R_r, P_r.rowRange(0, 3).colRange(0, 3), cv::Size(cols_r, rows_r), CV_32F, M1r,
                                M2r);

    // Create camera object
    setting::imageHeight = rows_l;
    setting::imageWidth = cols_l;
    setting::initSettings();
    float fx = fsSettings["Camera.fx"];
    float fy = fsSettings["Camera.fy"];
    float cx = fsSettings["Camera.cx"];
    float cy = fsSettings["Camera.cy"];
    float bf = fsSettings["Camera.bf"];

    shared_ptr<CameraParam> camera(new CameraParam(fx, fy, cx, cy, bf));
    this->mpCam = camera;

    srand(time(nullptr));
    // create a backend
    shared_ptr<BackendSlidingWindowG2O> backend(new BackendSlidingWindowG2O(pTracker));
    mpBackEnd = backend;

    // Main loop
    cv::Mat imLeft, imRight, imLeftRect, imRightRect;
    size_t imuIndex = 0;

    // set up visualization
    shared_ptr<Viewer> pviewer(new Viewer);
    this->mpViewer = pviewer;
    mbVisionOnlyMode = true;

    boost::format fmt("%s/%06d.jpg");

    ifstream timeFile(timeFolder);
    double initTime = 0;
    if (!timeFile) {
        LOG(INFO) << "Cannot find time file, I'll use fake timestamp instead. " << endl;
    } else {
        char buffer[256];
        timeFile.getline(buffer, 256);
        initTime = -atof(buffer);
    }

    // load the ground truth trajectory
    map<double, Vector3d, std::less<double>, Eigen::aligned_allocator<Vector3d>> trajectory;
    ifstream gtFile(gtTrajectory);
    if (!gtFile) {
        LOG(ERROR) << "Cannot find ground truth file! I won't show the ground-truth!" << endl;
    } else {
        while (!gtFile.eof()) {
            double time = 0;
            string useless;
            double xyz[3];
            double q[4];
            gtFile >> time >> useless
                   >> xyz[0] >> xyz[1] >> xyz[2]
                   >> q[0] >> q[1] >> q[2] >> q[3];
            if ( gtFile.good() == false || time == 0 )
                break;
            trajectory[time] = Vector3d(xyz[0], xyz[1], xyz[2]);
        }
        mpViewer->SetGTTraj(trajectory);
    }

    for (int ni = start_index; ni < nImages - start_index; ni++) {

        // Read left and right images from file
        string strLeft = (fmt % leftFolder % ni).str();
        string strRight = (fmt % rightFolder % ni).str();
        imLeft = cv::imread(strLeft, CV_LOAD_IMAGE_GRAYSCALE);
        imRight = cv::imread(strRight, CV_LOAD_IMAGE_GRAYSCALE);
        if (imLeft.empty() || imRight.empty()) {
            LOG(ERROR) << "Image not found." << endl;
            break;
        }

        cv::remap(imLeft, imLeftRect, M1l, M2l, cv::INTER_LINEAR);
        cv::remap(imRight, imRightRect, M1r, M2r, cv::INTER_LINEAR);

        VecIMU vimu;
        double time = 0;
        if (timeFile) {
            string strTime;
            timeFile >> strTime;
            time = std::atof(strTime.c_str());
            time += initTime;
        }
        SE3d Twb = this->InsertStereo(imLeftRect, imRightRect, time, vimu);
        LOG(INFO) << "current Twb = \n" << Twb.matrix() << endl;

        cv::Mat img = mpViewer->DrawImage();
        imshow("Image", img);
        cv::waitKey(1);

    }
    
    timeFile.close();
    sleep(2000);
    setting::destroySettings();
}
